import numpy as np

class Merge:
    def __init__(self,Traj):
        self.Traj=Traj
        self.TrajPoint=[]
    def run(self,tTable,refline_fun):
        t=0.1 # predict time
        s = self.Traj[2]["f"](t)
        l = self.Traj[1]["f"](s)
        start = 0
        end = len(tTable) - 1
        mid = (start + end) / 2
        targetpoint = 0
        while start <= end:
            if (tTable[mid]["s"] > s):
                end = mid - 1
            elif tTable[mid]["s"] < s:
                start = mid + 1
            else:
                start = mid
        rx = tTable[start]["x"]
        ry = tTable[start]["y"]
        rs = tTable[start]["s"]
        rtheta = np.polyder(refline_fun)(rx)
        return [rs,rx,ry,rtheta,s,l]